/* ######################################################################### */
/** \file isMovement.hpp

#include "ObjectFinder.hpp"
#include <math.h>
/* ------------------------------------------------------------------------- */
/**	\namespace DATMO
 *
 *  \brief	This is the namespace of the DATMO ROS NODE
 */
/* ------------------------------------------------------------------------- */

namespace DATMO
{
/* ------------------------------------------------------------------------- */
/**	\class isMovement
 *
 *  \brief  This is the class of isMovement function.
 *
 *
 */
/* ------------------------------------------------------------------------- */
class cl_isMovement
{
public:
	/* ------------------------------------------------------------------------- */
	/**	\fn public static void ObjectDetection()
	 *
	 *  \brief
	 *
	 *  \param[in]
	 *  \param[in]
	 *
	 *  \param[out] bool
	 *
	 */
	/* ------------------------------------------------------------------------- */
	cl_MovementData isMovement(auxMapCell LocalMapCell,
							cl_MovementData MovementData)
	{

		Matrix MovementMap(LocalMapCell.s_Nrows,LocalMapCell.s_Ncols);
		bool isMovement_global = false;
		bool isMovement_cell = false;
		int aux_i,aux_j;
		int u1[] = {0, 0, 1, 1, 1, 0, -1, -1, -1 };
		int u2[] = {0, 1, 1, 0, -1, -1, -1, 0, 1 };

		MovementMap = CompareReconst(LocalMapCell,MovementData.MapCellMatrix_k);

		return MovementData;
	}


	Matrix CompareReconst(auxMapCell LocalMapCell,auxMapCell LocalMapCell_ant)
	{
		double MIN_CONF = 2;
		bool isMovement = false;
		Matrix MovementMap(LocalMapCell.s_Nrows, LocalMapCell.s_Ncols);
		float Dif_X, Dif_Y;
		for (int i = 0; i < LocalMapCell.s_Nrows; i++)
		{
			for (int j = 0; j < LocalMapCell.s_Ncols; j++)
			{
				isMovement = false;
				if (LocalMapCell.Confidence(i, j) > MIN_CONF && LocalMapCell_ant.Confidence(i, j) > MIN_CONF)
				{
					//height check for differences more than 0.5 meters
					if ((Math.Abs(LocalMapCell.Measured_Heights(i, j) - LocalMapCell_ant.Measured_Heights(i, j)) > 0.5))
					{
						Dif_X = Math.Abs(LocalMapCell.Measured_Gradient_X(i,j)-LocalMapCell_ant.Measured_Gradient_X(i,j));
						Dif_Y = Math.Abs(LocalMapCell.Measured_Gradient_Y(i,j)-LocalMapCell_ant.Measured_Gradient_Y(i,j));
						if (Dif_X>0 && Dif_X<40 && Dif_Y>0 && Dif_Y<40)
							isMovement = true;
					}
				}
				if (isMovement)
				MovementMap(i, j) = 1;
			}
		}
		return MovementMap;
	}

};
}


//
//
// /* ------------------------------------------------------------------------- */
//        /**	\fn public static void isMovement()
//         *
//         *  \brief
//         *  \param[in] aux_MapCell LocalMapCell                 actual Mapcell
//         *  \param[in] aux_MapCell LocalMapCell_ant             last mapcell
//         *  \param[in] bool [,] ant_MovementMap                 Last movement map
//         *
//         *  \param[out]  isMovementStruct MovementData          MovementData structure with actual movement map and a variable indicating if there has been movement
//         */
//        /* ------------------------------------------------------------------------- */
//        public static isMovementStruct isMovement(aux_MapCell LocalMapCell,
//                                        isMovementStruct MovementData)
//        {
//            bool[,] MovementMap = new bool[LocalMapCell.us_Nrows,LocalMapCell.us_Ncols];
//            bool isMovement_global = false;
//            bool isMovement_cell = false;
//            int aux_i,aux_j;
//            int[] u1 = {0, 0, 1, 1, 1, 0, -1, -1, -1 };
//            int[] u2 = {0, 1, 1, 0, -1, -1, -1, 0, 1 };
//
//            MovementMap = M44_CompareReconst.CompareReconst(LocalMapCell, MovementData.MapCellMatrix_k);
//            //Check both movement maps for continuous movement
//            for (int i = 0; i < LocalMapCell.us_Nrows; i++)
//            {
//                for (int j = 0; j < LocalMapCell.us_Ncols; j++)
//                {
//                    MovementData.tb_MovementMap_k[i, j] = false;
//                    if (MovementMap[i, j])
//                    {
//                        isMovement_cell = false;
//                        for (int k = 0; k < 9; k++)
//                        {
//                            aux_i = i + u1[k];
//                            aux_j = j + u2[k];
//                            if (aux_i >= 0 && aux_i < LocalMapCell.us_Nrows &&
//                                aux_j >= 0 && aux_j < LocalMapCell.us_Ncols)
//                            {
//                                if (MovementData.tb_MovementMap_k_1[aux_i, aux_j])
//                                    isMovement_cell = true;
//                            }
//                        }
//                        if (isMovement_cell)
//                        {
//                            MovementData.tb_MovementMap_k[i, j] = true;
//                            isMovement_global = true;
//                        }
//                    }
//
//                }
//            }
//            MovementData.b_isMovement = isMovement_global;
//            MovementData.tb_MovementMap_k_1 = MovementMap;
//            MovementData.MapCellMatrix_k = LocalMapCell;
//
//            return MovementData;
//        }
